%% 例题7-1：针对平面2R机器人的力位混合控制器仿真例程
% 根据题义，假定两个关节电机和系统参数均相同的情况，因此，动力学方程计算公式不具有通用性
% 利用循环模拟控制系统时钟，假定每个采样周期都进行伺服控制，迭代计算末端加速度和力，且每次伺服控制都更新伺服期望值定时器
%% 程序启动，清理内存和绘图
clc;
clear;
clf;

%% 设定减速比
N = 10; 

%% 设定电机力矩上限
ia_limit = 2.5; % 设定电机电流上限为2.5A
Ka = 8.22e-2; % 电机力矩常数
Kg = 1; % 电流放大器增益
J_Tq_limit = ia_limit * Ka * Kg * N; % 设定关节力矩上限

%% 设定闭环系统响应特性
ts = 0.1;%调节时间
Zeta = 1;%阻尼比
if (Zeta == 1), Omega_n = 4.75/ts; end % 临界阻尼系统的自然频率
if (Zeta < 1), Omega_n = 3.5/(Zeta*ts); end % 欠阻尼系统的自然频率
if (Zeta > 1), Omega_n = 3.3/((Zeta- sqrt(Zeta^2-1))*ts); end % 过阻尼系统的自然频率

%% 设定机器人位置控制器增益矩阵
Kp = [Omega_n^2, 0; 0, Omega_n^2];
Kv = [2*Zeta*Omega_n, 0; 0, 2*Zeta*Omega_n];
Ki = [10, 0; 0, 10];

%% 设定与力位混合相关参数
% 力控制器增益矩阵
Kfp = [10, 0; 0, 10];
Kfv = [1000, 0; 0, 1000];
Kfi = [10, 0; 0, 10];
% 环境刚度矩阵
Kenv = [0 0; 0 1e5]; 
% 控制力映射矩阵
Cp = [1 0;0 1];
Cf = [0 0;0 1];

%% 设定模型偏差
D_m = 0.05; % 理论质量与实际质量的偏差率
D_L = 0.05; % 理论杆长与实际杆长的偏差率

%% 系统参数初始化
%% 单杆参数
g = 9.8;
m = 0.5; % 实际质量
L = 0.1; % 实际杆长
Br = 4.1e-4;
Ir = 1.19e-5;
Bl = 2e-2;
m_D = m * (1 - D_m); % 理论质量
L_D = L * (1 - D_L); % 理论杆长


% %% 平面2R机器人驱动空间参数矩阵，用于模拟真实关节运动
% % 平均等效质量矩阵，实际值
% M11_Average = (3*m*L^2)/N^2 + Ir; % 1#关节电机平均等效转动惯量
% M22_Average = (m*L^2)/N^2 + Ir; % 2#关节电机平均等效转动惯量
% Mm_Average = [M11_Average 0;0 M22_Average]; % 机器人驱动空间平均惯量矩阵
% % 等效阻尼矩阵
% Bm11 = Bl/N^2 + Br; % 1#关节电机平均等效转动惯量
% Bm22 = Bl/N^2 + Br; % 2#关节电机平均等效转动惯量
% Bm = [Bm11 0; 0 Bm22]; % 机器人驱动空间等效阻尼矩阵

%% 设置仿真时间、步长和步数
dt = 0.0001;
total_time = 4;
time = 0:dt:total_time; % 时间序列
step = total_time/dt + 1; % 步数

%% 初始化保存仿真计算结果的数组，均为2行、step列矩阵，行数代表关节编号，列数为迭代步数
J_theta = zeros(2,step); % 记录关节实际转角数组，输出图形的纵坐标轴
J_omega = zeros(2,step); % 记录关节实际转速，输出图形的纵坐标轴
J_epsilon = zeros(2,step); % 记录关节实际加速度，输出图形的纵坐标轴

End_Pos = zeros(2,step); % 记录末端实际位置数组，输出图形的纵坐标轴
End_Vol = zeros(2,step); % 记录末端实际转速，输出图形的纵坐标轴
End_Acc = zeros(2,step); % 记录末端实际加速度，输出图形的纵坐标轴

End_F = zeros(2,step); % 记录末端实际接触力，此为机器人对环境施加的力
%% 初始化仿真过程的中间变量
% 关节空间和驱动空间变量，均为2行、1列，行数代表关节编号
J_real_theta_pre = zeros(2,1); % 迭代计算前，当前关节实际转角
J_real_theta_aft = zeros(2,1); % 迭代计算后，当前关节实际转角
J_real_omega_pre = zeros(2,1); % 迭代计算前，当前关节实际转速
J_real_omega_aft = zeros(2,1); % 迭代计算后，当前关节实际转速
J_real_epsilon_pre = zeros(2,1); % 迭代计算后，当前关节实际加速度
J_real_epsilon_aft = zeros(2,1); % 迭代计算后，当前关节实际加速度
M_real_theta_pre = zeros(2,1); % 迭代计算前，当前电机实际转角
M_real_theta_aft = zeros(2,1); % 迭代计算后，当前电机实际转角
M_real_omega_pre = zeros(2,1); % 迭代计算前，当前电机实际转速
M_real_omega_aft = zeros(2,1); % 迭代计算后，当前电机实际转速
M_real_epsilon_pre = zeros(2,1); % 迭代计算后，当前电机实际加速度
M_real_epsilon_aft = zeros(2,1); % 迭代计算后，当前电机实际加速度

% 操作空间变量，均为2行、1列，行数代表X、Y轴
End_Pos_pre = zeros(2,1); % 迭代计算前，末端位置
End_Pos_aft = zeros(2,1); % 迭代计算后，末端位置
End_Vol_pre = zeros(2,1); % 迭代计算前，末端速度
End_Vol_aft = zeros(2,1); % 迭代计算后，末端速度
End_Acc_pre = zeros(2,1); % 迭代计算前，末端加速度
End_Acc_aft = zeros(2,1); % 迭代计算后，末端加速度
End_F_pre = zeros(2,1); % 迭代计算前，末端接触力
End_F_aft = zeros(2,1); % 迭代计算前，末端接触力


% 根据题义设定起始的实际位姿，包括末端位姿和关节转角
End_Pos_pre(1,1) = 0.1;
End_Pos_aft(1,1)= 0.1;
J_real_theta_pre(1,1) = pi/3;
J_real_theta_aft(1,1) = pi/3;
J_real_theta_pre(2,1) = -2*pi/3;
J_real_theta_aft(2,1) = -2*pi/3;


Fxc = zeros(2,1); % 操作空间控制力
Fxpc = zeros(2,1); % 位置控制部分操作空间控制力
Fxfc = zeros(2,1); % 力控制部分操作空间控制力
Fxcc = zeros(2,1); % 操作空间非线性控制力

Fxd = zeros(2,1); % 操作空间干扰力


% J_Tqc = zeros(2,1); % 控制器输出的关节空间控制力矩
% M_Tqd = zeros(2,1); % 等效到电机转子的扰动力矩
% M_Tqa = zeros(2,1); % 作用于电机转子的等效力矩

End_Pos_E = zeros(2,1); % 末端实际位置与期望位置误差
End_Vol_E = zeros(2,1); % 末端实际速度与期望速度误差
End_F_E = zeros(2,1); % 末端实际接触力与期望接触力误差
End_F_E_integral  = zeros(2,1); % 末端力误差积分
End_Pos_E_integral = zeros(2,1); % 末端位置误差积分

%% 初始化关节空间动力学参数矩阵
% 实际参数
J_M = [0,0;0,0];
J_V = [0,0;0,0];
J_B = [0,0;0,0];
J_G = [0;0];
% 理论参数
J_M_D = [0,0;0,0];
J_V_D = [0,0;0,0];
J_B_D = [0,0;0,0];
J_G_D = [0;0];
% 雅可比矩阵及其微分
Jacobi = [0,0;0,0]; % 雅可比实际参数
Jacod = [0,0;0,0]; % 雅可比微分实际参数
Jacobi_D = [0,0;0,0]; % 雅可比理论参数
dJacobi_D = [0,0;0,0]; % 雅可比微分理论参数

%% 初始化操作空间动力学参数矩阵
% 实际参数
Mx = [0,0;0,0];
Nx = [0,0;0,0];
Gx = [0;0];
% 理论参数
Mx_D = [0,0;0,0];
Nx_D = [0,0;0,0];
Gx_D = [0;0];

%% 设置加速度、速度和位置期望值
[End_Pos_set(1,:), End_Vol_set(1,:), End_Acc_set(1,:)] = Position_S_traj_7_1(total_time,dt,2.5e-2);
End_Pos_set(2,:) = zeros(1,step);
End_Vol_set(2,:) = zeros(1,step);
End_Acc_set(2,:) = zeros(1,step);
End_F_set = zeros(2,step);
End_F_set(2,:) = -10; % 期望接触力X方向为0，Y方向为-10N

%% 主循环
for i = 1:step

    %% 根据连杆理论值和当前实际位置、速度更新操作空间动力学理论参数矩阵和非线性控制力
    [J_M_D, J_V_D, J_B_D, J_G_D, Jacobi_D, dJacobi_D, Inv_Jacobi_D] = update_matrix_7_1(m_D, L_D, g, N, Ir, Bl, Br, J_real_theta_pre(1,1),J_real_theta_pre(2,1),J_real_omega_pre(1,1),J_real_omega_pre(2,1)); % 更新动力学矩阵
    Mx_D = Inv_Jacobi_D' * J_M_D * Inv_Jacobi_D;
    Nx_D = Inv_Jacobi_D' * (J_V_D + J_B_D - J_M_D * Inv_Jacobi_D * dJacobi_D) * Inv_Jacobi_D;
    Gx_D = Inv_Jacobi_D' * J_G_D;
    Fxcc = Nx_D * End_Vol_pre + Gx_D; % 操作空间非线性控制力  
    
    %% 获得操作空间的控制期望值
    End_Pos_d = End_Pos_set(:,i); % 期望末端位置
    End_Vol_d = End_Vol_set(:,i); % 期望末端速度
    End_Acc_d = End_Acc_set(:,i); % 期望末端加速度 
    End_F_d = End_F_set (:,i); % 期望末端接触力
    
    %% 计算末端位置和速度误差
    End_Pos_E = End_Pos_d - End_Pos_pre; % 位置误差
    End_Vol_E = End_Vol_d - End_Vol_pre; % 速度误差
    End_Pos_E_integral = End_Pos_E_integral + End_Pos_E .* dt; % 位置误差的积分
    
    %% 计算末端力误差
    End_F_E = End_F_d - End_F_pre; % 力误差
    End_F_E_integral = End_F_E_integral + End_F_E .* dt; % 力误差的积分
    
    %% 调用位置逆动力学PID控制器，得到操作空间位置控制力
    PID_Acc = End_Acc_d + Kp * End_Pos_E + Kv * End_Vol_E + Ki * End_Pos_E_integral; % 指令加速度
    Fxpc = Cp * (Mx_D * PID_Acc); % 位置控制器输出的控制力

    %% 调用力PI控制器，得到操作空间力控制力
    End_F_E = End_F_d - End_F_pre;
    End_F_E_integral = End_F_E_integral + End_F_E .* dt;
    
    Fxfc = Cf * (End_F_d + Kfp * End_F_E + Kfi * End_F_E_integral - Kfv * End_Vol_pre); % 力控制器输出的控制力

    %% 计算力位混合控制器的总控制力
    Fxc = Fxpc + Fxfc + Fxcc;

    %% 根据动力学方程迭代更新末端位置、速度和加速度
    %% 利用操作空间动力学方程直接计算末端加速度
    % 根据连杆实际值和当前实际位置、速度更新动力学实际参数矩阵
    [J_M, J_V, J_B, J_G, Jacobi, dJacobi, Inv_Jacobi] = update_matrix_7_1(m, L, g, N, Ir, Bl, Br, J_real_theta_pre(1,1),J_real_theta_pre(2,1),J_real_omega_pre(1,1),J_real_omega_pre(2,1)); % 更新动力学矩阵
    Mx = Inv_Jacobi' * J_M * Inv_Jacobi;
    Nx = Inv_Jacobi' * (J_V + J_B - J_M * Inv_Jacobi * dJacobi) * Inv_Jacobi;
    Gx = Inv_Jacobi' * J_G;

    Fxd = Nx * End_Vol_pre + Gx; % 操作空间干扰力

    End_Acc_aft = Mx \ (Fxc - Fxd - End_F_pre); % 当前末端加速度，注意，这里要考虑末端接触力的作用
    
    End_Vol_aft = End_Vol_pre + End_Acc_aft .* dt; % 当前末端速度
    End_Pos_aft = End_Pos_pre + End_Vol_aft .* dt; % 当前末端位置

    if ( End_Pos_aft(2,1) <= 0 ) 
        End_F_aft = Kenv * End_Pos_aft; % 如果Y方向压缩接触面,根据环境刚度计算末端接触力
    else
        End_F_aft = [0;0]; % 如果Y方向脱离接触面，接触力置为零
    end
    
    J_real_omega_aft = Inv_Jacobi * End_Vol_aft; % 当前关节速度
    J_real_epsilon_aft = (J_real_omega_aft - J_real_omega_pre) ./ dt; % 当前关节加速度
    J_real_theta_aft = J_real_theta_pre + J_real_omega_aft .* dt; % 当前关节位置
    
    %% 记录状态输出数据并更新迭代前状态值
    End_Pos(:,i) = End_Pos_aft; % 记录末端位置
    End_Vol(:,i) = End_Vol_aft; % 记录末端速度
    End_Acc(:,i) = End_Acc_aft; % 记录末端加速度
    End_F(:,i) = End_F_aft; % 记录末端力
       
    J_theta(:,i) =  J_real_theta_aft; % 记录当前关节角度
    J_omega(:,i) =  J_real_omega_aft; % 记录当前关节角速度
    J_epsilon(:,i) =  J_real_epsilon_aft; % 记录当前关节角加速度
    
    % 更新迭代前的末端位置、速度和加速度
    End_Pos_pre = End_Pos_aft; % 更新末端位置
    End_Vol_pre = End_Vol_aft; % 更新末端速度
    End_Acc_pre = End_Acc_aft; % 更新末端加速度
    End_F_pre = End_F_aft; % 更新末端力
    
    J_real_theta_pre = J_real_theta_aft; % 更新迭代前关节实际转角
    J_real_omega_pre = J_real_omega_aft; % 更新迭代前关节实际转速
    J_real_epsilon_pre = J_real_epsilon_aft; % 更新迭代前关节实际角加速度      

    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
    % 需要结合真实的电流放大器——电机环节进一步仿真
%     tm(:,i) = J' * Fxc; % 关节空间控制力矩
%     td(:,i) = J' * Fxd; % 关节空间干扰力矩
     
%     if (J_Tqc(1,1) > J_Tq_limit),T_qc(1,1) = J_Tq_limit; end % 控制力矩上限
%     if (J_Tqc(2,1) > J_Tq_limit),T_qc(2,1) = J_Tq_limit; end % 控制电压上限
%     if (J_Tqc(1,1) < -J_Tq_limit),Uc(1,1) = -J_Tq_limit; end % 控制电压上限
%     if (J_Tqc(2,1) < -J_Tq_limit),Uc(2,1) = -J_Tq_limit; end % 控制电压上限
    
%     %% 更新迭代前的电机和关节状态变量
%     M_real_theta_pre = M_real_theta_aft; % 更新迭代前电机实际转角
%     M_real_omega_pre = M_real_omega_aft; % 更新迭代前电机实际转速
%     M_real_epsilon_pre = M_real_epsilon_aft; % 更新迭代前电机实际角加速度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    
end

%% 绘图
%% 末端X方向位置指令曲线
% figure(1);
% yyaxis left;
% % ylabel('x(t)');
% p = plot(time,End_Pos_set(1,:),'k-');
% p.LineWidth=1.5;
% ylim([0.095,0.155]);
% set(gca,'ycolor','k');
% yyaxis right;
% % ylabel('y(t)');
% p = plot(time,End_Vol_set(1,:),'b--');
% p.LineWidth = 1.5;
% ylim([-5e-3,30e-3]);
% set(gca,'ycolor','k');
% % xlabel('t');
% title(sprintf("位置指令曲线"));
%% 末端Y方向位置指令曲线
% figure(2);
% yyaxis left;
% % ylabel('x(t)');
% p = plot(time,End_Pos_set(2,:),'k-');
% p.LineWidth=1.5;
% % ylim([0.095,0.155]);
% set(gca,'ycolor','k');
% yyaxis right;
% % ylabel('y(t)');
% p = plot(time,End_Vol_set(2,:),'b--');
% p.LineWidth = 1.5;
% set(gca,'ycolor','k');
% % xlabel('t');
% % title(sprintf("位置指令曲线"));
%% 末端力指令曲线
% figure(3);
% yyaxis left;
% % ylabel('Fx(t)');
% p = plot(time,End_F_set(1,:),'k-');
% p.LineWidth=1.5;
% ylim([-20,20]);
% set(gca,'ycolor','k');
% yyaxis right;
% % ylabel('Fy(t)');
% p = plot(time,End_F_set(2,:),'b--');
% p.LineWidth = 1.5;
% ylim([-20,20]);
% set(gca,'ycolor','k');
% % xlabel('t');
% % title(sprintf("位置指令曲线"));
%% 末端X方向位置响应曲线
figure(4);
yyaxis left;
% ylabel('x(t)');
p=plot(time,End_Pos(1,:),'k-',time,End_Pos_set(1,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
ylim([0.095,0.155]);
set(gca,'ycolor','k');
yyaxis right;
% ylabel('x_e(t)');
p = plot(time, End_Pos_set(1,:) - End_Pos(1,:), 'r-.');
p.LineWidth = 0.8;
ylim([-4e-5,4e-5]);
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("末端X方向响应曲线"));
%% 末端Y方向位置响应曲线
figure(5);
yyaxis left;
% ylabel('y(t)');
p=plot(time,End_Pos(2,:),'k-',time,End_Pos_set(2,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
ylim([-1e-3,1e-3]);
set(gca,'ycolor','k');
yyaxis right;
% ylabel('y_e(t)');
p = plot(time, End_Pos_set(2,:) - End_Pos(2,:), 'r-.');
p.LineWidth = 0.8;
ylim([-8e-5,8e-5]);
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("末端Y方向响应曲线"));
%% 末端Y方向力响应曲线
figure(6);
yyaxis left;
% ylabel('Fy(t)');
p=plot(time,End_F(2,:),'k-',time,End_F_set(2,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
% ylim([-1e-3,1e-3]);
set(gca,'ycolor','k');
yyaxis right;
% ylabel('Fy_e(t)');
p = plot(time, End_F_set(2,:) - End_F(2,:), 'r-.');
p.LineWidth = 0.8;
% ylim([-8e-5,8e-5]);
set(gca,'ycolor','k');
% xlabel('t');
% title(sprintf("末端Y方向响应曲线"));